﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Sharp3D.Math.Core;

namespace GLWrap.MathExt
{
    public static class Matrix4FExtension
    {
        public static Matrix4F RotationMatrix(float angle, Vector3F axis)
        {
            float s = (float)Math.Sin(angle / 180.0f * 3.14f);
            float c = (float)Math.Cos(angle / 180.0f * 3.14f);
            return new Matrix4F(
                c + axis.X*axis.X*(1-c),      axis.X*axis.Y*(1-c)-axis.Z*s,  axis.X*axis.Z*(1-c)+axis.Y*s, 0,
                axis.Y*axis.X*(1-c)+axis.Z*s, c+axis.Y*axis.Y*(1-c),         axis.Y*axis.Z*(1-c)-axis.X*s, 0,
                axis.Z*axis.X*(1-c)-axis.Y*s, axis.Z*axis.Y*(1-c)+ axis.X*s, c + axis.Z*axis.Z*(1-c),      0,
                0,0,0,1
                );
        }

        public static Matrix4F TranslateMatrix(Vector3F v)
        {
            Matrix4F result = Matrix4F.Identity;
            result.M14 = v.X;
            result.M24 = v.Y;
            result.M34 = v.Z;
            return result;
        }

        public static Vector3F Multiply(this Matrix4F m, Vector3F v)
        {
            float[] vector_array = new[] { v.X,v.Y,v.Z,1};
            Vector3F result = new Vector3F(0,0,0);
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 4; j++)
                    result[i] += m[i+1, j+1] * vector_array[j];
            }
            return result;
        }

        public static Matrix4F Multiply(this Matrix4F m, float v)
        {
            for (int i = 0; i < 16; i++) m[i] *= v;
            return m;
        }

        public static float[] TranslateToGL(this Matrix4F m)
        {
            return new[] { m[0], m[4], m[8], m[12], m[1], m[5], m[9], m[13], m[2], m[6], m[10], m[14], m[3], m[7], m[11], m[15]};
        }

        public static QuaternionF GetQuaternion(this Matrix4F m)
        {
            QuaternionF result = new QuaternionF();
            
        float tr = m[1,1] + m[2,2] + m[3,3]; // trace of martix
        if (tr > 0.0f){     // if trace positive than "w" is biggest component
            
            
             result.X =  m[2,3] - m[3,2];
            result.Y = m[3,1] - m[1,3];
            result.Z = m[1,2] - m[2,1];
            result.W = tr+1.0f;
             result *=  0.5f/(float) Math.Sqrt( result.W );     // "w" contain the "norm * 4"

        }else                 // Some of vector components is bigger
        if( (m[1,1] > m[2,2] ) && ( m[1,1] > m[3,3]) ) {
            result.X =  1.0f + m[1,1] - m[2,2] - m[3,3];
            result.Y = m[2,1] + m[1,2];
            result.Z = m[3,1] + m[1,3];
            result.W = m[2,3] - m[3,2];
            result *= ( 0.5f/(float)Math.Sqrt( result.X ) );

        }else 
        if ( m[2,2] > m[3,3] ){
            result.X =  m[2,1] + m[1,2];
            result.Y = 1.0f + m[2,2] - m[1,1] - m[3,3];
                 result.Z = m[3,2] + m[2,3];
            result.W =m[3,1] - m[1,3] ; 
            result *= ( 0.5f/(float)Math.Sqrt( result.Y ) );

        }else{
            result.X =  m[3,1] + m[1,3];
            result.Y = m[3,2] + m[2,3];
            result.Z = 1.0f + m[3,3] - m[1,1] - m[2,2];
            result.W = m[1,2] - m[2,1];
            result *= ( 0.5f/(float)Math.Sqrt( result.Z ) );

        }
        return result;
        }
    }
}
